﻿using System;
using System.Collections.Generic;
using Entitas;
using UnityEngine;
using Random = System.Random;

public class SteerBehaviorSystem : IExecuteSystem
{
    public void Execute()
    {
        var entities = Contexts.sharedInstance.game
            .GetGroup(GameMatcher.AllOf(GameMatcher.Behavior, GameMatcher.Accelerate, GameMatcher.Transform,
                GameMatcher.Velocity))
            .GetEntities();

        foreach (var e in entities)
        {
            GameEntity target = null;
            List<GameEntity> neighbors = null;

            if (On(e.behavior.behaviors, BehaviorType.Separation |
                                         BehaviorType.Alignment |
                                         BehaviorType.Cohesion |
                                         BehaviorType.ObstacleAvoidance))
            {
                neighbors = Contexts.sharedInstance.game.cellSpace.cellSpace.GetNeighbors(e.transform.position,
                    GameDefine.QUERY_NEIGHBOR_RADIUS);
            }


            Vector3 force = Vector3.zero;
            if (On(e.behavior.behaviors, BehaviorType.WallAvoidance))
            {
                var walls = Contexts.sharedInstance.game
                    .GetGroup(GameMatcher.AllOf(GameMatcher.Wall))
                    .GetEntities();
                Vector3 add = WallAvoidance(e, new List<GameEntity>(walls)) * GameDefine.WALL_AVOIDANCE_WEIGHT;
                if (!AccumulateForce(ref force, add, e.accelerate.max))
                {
                    e.accelerate.value = force;
                    e.accelerate.Truncate();
                    continue;
                }
            }

            if (On(e.behavior.behaviors, BehaviorType.ObstacleAvoidance))
            {
                Vector3 add = ObstacleAvoidance(e, neighbors) * GameDefine.OBSTACLE_AVOIDANCE_WEIGHT;
                if (!AccumulateForce(ref force, add, e.accelerate.max))
                {
                    e.accelerate.value = force;
                    e.accelerate.Truncate();
                    continue;
                }
            }

            if (On(e.behavior.behaviors, BehaviorType.Evade))
            {
                Vector3 add = Evade(e, target) * GameDefine.EVADE_WEIGHT;
                if (!AccumulateForce(ref force, add, e.accelerate.max))
                {
                    e.accelerate.value = force;
                    e.accelerate.Truncate();
                    continue;
                }
            }

            if (On(e.behavior.behaviors, BehaviorType.Flee) && target != null)
            {
                Vector3 add = Flee(e, target.transform.position);
                if (!AccumulateForce(ref force, add, e.accelerate.max))
                {
                    e.accelerate.value = force;
                    e.accelerate.Truncate();
                    continue;
                }
            }

            if (On(e.behavior.behaviors, BehaviorType.Separation))
            {
                Vector3 add = Separation(e, neighbors) * GameDefine.SEPARATE_WEIGHT;
                if (!AccumulateForce(ref force, add, e.accelerate.max))
                {
                    e.accelerate.value = force;
                    e.accelerate.Truncate();
                    continue;
                }
            }

            if (On(e.behavior.behaviors, BehaviorType.Alignment))
            {
                Vector3 add = Alignment(e, neighbors) * GameDefine.ALIGNMENT_WEIGHT;
                if (!AccumulateForce(ref force, add, e.accelerate.max))
                {
                    e.accelerate.value = force;
                    e.accelerate.Truncate();
                    continue;
                }
            }

            if (On(e.behavior.behaviors, BehaviorType.Cohesion))
            {
                Vector3 add = Cohesion(e, neighbors) * GameDefine.COHESION_WEIGHT;
                if (!AccumulateForce(ref force, add, e.accelerate.max))
                {
                    e.accelerate.value = force;
                    e.accelerate.Truncate();
                    continue;
                }
            }

            if (On(e.behavior.behaviors, BehaviorType.Seek) && target != null)
            {
                Vector3 add = Seek(e, target.transform.position);
                if (!AccumulateForce(ref force, add, e.accelerate.max))
                {
                    e.accelerate.value = force;
                    e.accelerate.Truncate();
                    continue;
                }
            }

            if (On(e.behavior.behaviors, BehaviorType.Arrive) && target != null)
            {
                Vector3 add = Arrive(e, target.transform.position);
                if (!AccumulateForce(ref force, add, e.accelerate.max))
                {
                    e.accelerate.value = force;
                    e.accelerate.Truncate();
                    continue;
                }
            }

            if (On(e.behavior.behaviors, BehaviorType.Wander))
            {
                Vector3 add = Wander(e);
                if (!AccumulateForce(ref force, add, e.accelerate.max))
                {
                    e.accelerate.value = force;
                    e.accelerate.Truncate();
                    continue;
                }
            }

            if (On(e.behavior.behaviors, BehaviorType.Pursuit))
            {
                Vector3 add = Pursuit(e, target);
                if (!AccumulateForce(ref force, add, e.accelerate.max))
                {
                    e.accelerate.value = force;
                    e.accelerate.Truncate();
                    continue;
                }
            }

            if (On(e.behavior.behaviors, BehaviorType.OffsetPursuit))
            {
                Vector3 add = OffsetPursuit(e, target, e.behavior.offset);
                if (!AccumulateForce(ref force, add, e.accelerate.max))
                {
                    e.accelerate.value = force;
                    e.accelerate.Truncate();
                    continue;
                }
            }

            if (On(e.behavior.behaviors, BehaviorType.Interpose))
            {
                Vector3 add = Interpose(e, neighbors[0], neighbors[1]);
                if (!AccumulateForce(ref force, add, e.accelerate.max))
                {
                    e.accelerate.value = force;
                    e.accelerate.Truncate();
                    continue;
                }
            }

            if (On(e.behavior.behaviors, BehaviorType.Hide))
            {
                Vector3 add = Hide(e, target, neighbors);
                if (!AccumulateForce(ref force, add, e.accelerate.max))
                {
                    e.accelerate.value = force;
                    e.accelerate.Truncate();
                    continue;
                }
            }

            if (On(e.behavior.behaviors, BehaviorType.FollowPath))
            {
                Vector3 add = FollowPath(e) * GameDefine.FOLLOWPATH_WEIGHT;
                if (!AccumulateForce(ref force, add, e.accelerate.max))
                {
                    e.accelerate.value = force;
                    e.accelerate.Truncate();
                    continue;
                }
            }

            e.accelerate.value = force;
            e.accelerate.Truncate();
        }
    }

    private bool AccumulateForce(ref Vector3 force, Vector3 add, float maxForce)
    {
        float f = force.magnitude;
        float remain = maxForce - f;
        if (remain <= 0)
        {
            return false;
        }

        float addF = add.magnitude;
        if (addF < remain)
        {
            force += add;
        }
        else
        {
            force += add.normalized * remain;
        }

        return true;
    }

    private bool On(BehaviorType behaviourTypes, BehaviorType behaviourType)
    {
        return (behaviourTypes & behaviourType) > 0;
    }

    private Vector3 Seek(GameEntity agent, Vector3 targetPos)
    {
        Vector3 desireVelocity =
            (targetPos - agent.transform.position).normalized * agent.velocity.max;

        return desireVelocity - agent.velocity.value;
    }

    private Vector3 Flee(GameEntity agent, Vector3 targetPos)
    {
        Vector3 desireVelocity =
            (agent.transform.position - targetPos).normalized * agent.velocity.max;

        return desireVelocity - agent.velocity.value;
    }

    private Vector3 Arrive(GameEntity agent, Vector3 targetPos)
    {
        Vector3 toTarget = targetPos - agent.transform.position;
        float dist = toTarget.magnitude;
        if (dist > 0f)
        {
            float speed = dist / 2;
            speed = Math.Min(agent.velocity.max, speed);
            Vector3 desireVelocity = speed * toTarget / dist;
            return desireVelocity - agent.velocity.value;
        }

        return Vector3.zero;
    }

    private Vector3 Pursuit(GameEntity agent, GameEntity target)
    {
        Vector3 toTarget = target.transform.position - agent.transform.position;
        float relativeForward = Vector3.Dot(agent.transform.forward, target.transform.forward);
        float dot = Vector3.Dot(toTarget.normalized, agent.transform.forward.normalized);
        if (dot > 0 && relativeForward < -0.95f)
        {
            return Seek(agent, target.transform.position);
        }

        float lookAheadTime =
            toTarget.magnitude / (agent.velocity.max + target.velocity.value.magnitude);

        return Seek(agent, target.transform.position + target.velocity.value * lookAheadTime);
    }


    private Vector3 Evade(GameEntity agent, GameEntity target)
    {
        Vector3 toTarget = target.transform.position - agent.transform.position;
        float lookAheadTime =
            toTarget.magnitude / (agent.velocity.max + target.velocity.value.magnitude);

        return Flee(agent, target.transform.position + target.velocity.value * lookAheadTime);
    }

    private Vector3 Wander(GameEntity agent)
    {
        WanderParams wanderParams = agent.behavior.wanderParams;
        if (wanderParams == null)
        {
            return Vector3.zero;
        }

        Random random = Contexts.sharedInstance.game.random.random;
        wanderParams.wanderTarget += new Vector3(random.Next(-10000, 10000) / 10000f * wanderParams.wanderJitter, 0,
            random.Next(-10000, 10000) / 10000f * wanderParams.wanderJitter);
        wanderParams.wanderTarget.Normalize();

        wanderParams.wanderTarget *= wanderParams.wanderRadius;

        Vector3 targetLocal = wanderParams.wanderTarget + new Vector3(0, 0, wanderParams.wanderDistance);

        Vector3 targetWorld = agent.transform.LocalToWorld(targetLocal);

        return targetWorld - agent.transform.position;
    }

    private Vector3 ObstacleAvoidance(GameEntity agent, List<GameEntity> obstacles)
    {
        float minBoxLength = GameDefine.MIN_BOX_LENGTH;

        VelocityComponent velocity = agent.velocity;
        float boxLength = minBoxLength + velocity.value.magnitude / velocity.max * minBoxLength;
        Vector3 localPosOfClosestObstacle = Vector3.one;
        GameEntity closestIntersectingObstacle = null;
        float distToClosestIP = float.MaxValue;
        foreach (var obstacle in obstacles)
        {
            if (!obstacle.hasColliderRadius)
            {
                continue;
            }
            
            Vector3 localPos = agent.transform.WorldToLocal(obstacle.transform.position);
            if (localPos.z < 0)
            {
                continue;
            }

            if (localPos.z + obstacle.colliderRadius.radius > boxLength)
            {
                continue;
            }

            float expandedRadius = agent.colliderRadius.radius + obstacle.colliderRadius.radius;
            if (Math.Abs(localPos.x) >= expandedRadius)
            {
                continue;
            }

            float cx = localPos.x;
            float cz = localPos.z;

            float sqrt = (float) Math.Sqrt(expandedRadius * expandedRadius - cx * cx);
            float ip = cz - sqrt;
            if (ip <= 0)
            {
                ip = cz + sqrt;
            }

            if (ip < distToClosestIP)
            {
                distToClosestIP = ip;
                closestIntersectingObstacle = obstacle;
                localPosOfClosestObstacle = localPos;
            }
        }

        if (closestIntersectingObstacle == null)
        {
            return Vector3.zero;
        }

        float multiplier = 1.0f + (boxLength - localPosOfClosestObstacle.z) / boxLength;
        float forceX = -localPosOfClosestObstacle.x * multiplier;

        float forceZ = (closestIntersectingObstacle.colliderRadius.radius - localPosOfClosestObstacle.z) *
                       GameDefine.BRAKING_WEIGHT;

        return agent.transform.LocalToWorld(new Vector3(forceX, 0, forceZ));
    }

    private Vector3 Interpose(GameEntity agent, GameEntity targetA, GameEntity targetB)
    {
        Vector3 midPos = (targetA.transform.position + targetB.transform.position) / 2f;
        float t = (midPos - agent.transform.position).magnitude / agent.velocity.max;
        Vector3 predictA = targetA.transform.position + targetA.velocity.value * t;
        Vector3 predictB = targetB.transform.position + targetB.velocity.value * t;
        Vector3 targetPos = (predictA + predictB) / 2f;

        return Arrive(agent, targetPos);
    }

    private Vector3 GetHidingPosition(Vector3 obstaclePos, float obstacleRadius, Vector3 targetPos)
    {
        float distAway = GameDefine.HIDE_DISTANCE_FROM_BOUNDARY + obstacleRadius;
        Vector3 toOb = (obstaclePos - targetPos).normalized;
        return obstaclePos + toOb * distAway;
    }

    private Vector3 Hide(GameEntity agent, GameEntity target, List<GameEntity> obstacles)
    {
        float distToClosest = float.MaxValue;
        Vector3 bestHidingPos = Vector3.zero;

        Vector3 targetPos = target.transform.position;
        Vector3 agentPos = agent.transform.position;
        foreach (var obstacle in obstacles)
        {
            if (obstacle == target)
            {
                continue;
            }

            Vector3 hidingPos =
                GetHidingPosition(obstacle.transform.position, obstacle.colliderRadius.radius, targetPos);
            float sqrDist = (hidingPos - agentPos).sqrMagnitude;

            if (sqrDist < distToClosest)
            {
                distToClosest = sqrDist;
                bestHidingPos = hidingPos;
            }
        }

        if (distToClosest >= float.MaxValue)
        {
            return Evade(agent, target);
        }

        return Arrive(agent, bestHidingPos);
    }

    private Vector3 FollowPath(GameEntity agent)
    {
        Path path = agent.behavior.path;

        if ((path.CurrentWayPoint() - agent.transform.position).sqrMagnitude < GameDefine.WAY_POINT_SEEK_DIST_SQ)
        {
            path.NextWayPoint();
        }

        if (!path.Finished())
        {
            return Seek(agent, path.CurrentWayPoint());
        }
        else
        {
            return Arrive(agent, path.CurrentWayPoint());
        }
    }

    private Vector3 OffsetPursuit(GameEntity agent, GameEntity leader, Vector3 offset)
    {
        Vector3 worldOffsetPos = leader.transform.LocalToWorld(offset);

        Vector3 toOffset = worldOffsetPos - agent.transform.position;

        float lookAheadTime =
            toOffset.magnitude / (agent.velocity.max + leader.velocity.value.magnitude);

        return Arrive(agent, worldOffsetPos + lookAheadTime * leader.velocity.value);
    }

    private Vector3 WallAvoidance(GameEntity agent, List<GameEntity> walls)
    {
        float wallDelectionFeelerLength = GameDefine.WALL_DELECTION_FEELER_LENGTH;
        Vector3[] fleers = new Vector3[3];
        fleers[0] = agent.transform.position + agent.transform.forward * wallDelectionFeelerLength;
        // left
        fleers[1] = agent.transform.position + Quaternion.AngleAxis(-30, Vector3.up) *
            agent.transform.forward * wallDelectionFeelerLength / 2f;
        // right
        fleers[2] = Quaternion.AngleAxis(30, Vector3.up) *
            agent.transform.forward * wallDelectionFeelerLength / 2f;

        float distToThisIp = 0.0f;
        float distToClosestIp = float.MaxValue;
        Vector3 force = Vector3.zero;
        Vector3 point = Vector3.zero;
        Vector3 closestPoint = Vector3.zero;

        foreach (var fleer in fleers)
        {
            int closestWall = -1;
            for (int i = 0; i < walls.Count; i++)
            {
                WallComponent w = walls[i].wall;
                if (Geometry.LineIntersectionXZ(agent.transform.position,
                    fleer,
                    w.from,
                    w.to,
                    ref distToThisIp,
                    ref point
                ))
                {
                    if (distToThisIp < distToClosestIp)
                    {
                        distToClosestIp = distToThisIp;
                        closestWall = i;
                        closestPoint = point;
                    }
                }
            }

            if (closestWall >= 0)
            {
                Vector3 overShoot = fleer - closestPoint;
                force += walls[closestWall].wall.normal * overShoot.magnitude;
            }
        }

        return force;
    }

    private Vector3 Separation(GameEntity agent, List<GameEntity> neighbors)
    {
        Vector3 force = Vector3.zero;
        foreach (var n in neighbors)
        {
            if (agent == n)
            {
                continue;
            }

            Vector3 toAgent = agent.transform.position - n.transform.position;
            float length = toAgent.magnitude;
            if (length > 0)
            {
                force += toAgent.normalized / toAgent.magnitude;
            }
        }

        return force;
    }

    private Vector3 Cohesion(GameEntity agent, List<GameEntity> neighbors)
    {
        Vector3 centerPos = Vector3.zero;
        Vector3 force = Vector3.zero;
        int count = 0;
        foreach (var n in neighbors)
        {
            if (agent == n)
            {
                continue;
            }

            count++;
            centerPos += n.transform.position;
        }

        if (count > 0)
        {
            centerPos /= count;
            force = Seek(agent, centerPos);
        }

        return force;
    }

    private Vector3 Alignment(GameEntity agent, List<GameEntity> neighbors)
    {
        int count = 0;
        Vector3 averageForward = Vector3.zero;
        foreach (var n in neighbors)
        {
            if (agent == n)
            {
                continue;
            }

            count++;
            averageForward += n.transform.forward;
        }

        if (count > 0)
        {
            averageForward /= count;
            averageForward -= agent.transform.forward;
        }

        return averageForward;
    }
}